#include "CORD2RData.h"

#include <assert.h>

#include "DataStructure/Common/Validation.h"

using SG::Algebra::Point;
using SG::Algebra::Vector3D;
using SG::DataStructure::Common::Real;
using SG::DataStructure::FEM::Cartesian2Coord;

/** @brief 单元的每个节点定义在世界坐标系或者 节点所在的参考坐标系已经被初始化
 *
 */
SG::DataStructure::FEM::CoordStatus Cartesian2Coord::QueryStatus (ICoordinatePArray& coordSet) const
{
    if (Status_Initialized == m_status)
        return Status_Initialized;

    auto fund = (m_ref == 0) || (coordSet[m_ref]->QueryStatus (coordSet) == Status_Initialized);
    if (!fund)
        return Status_NeedOther;

    return Status_Ready;
}

void Cartesian2Coord::SetRefCoordId (std::size_t index, SG::DataStructure::Common::Id refId)
{
    assert (index < 1);
    m_ref = refId;
}

void Cartesian2Coord::GetRefPoint (_OUT SG::DataStructure::Common::Id& GA,
                                   _OUT SG::DataStructure::Common::Id& GB,
                                   _OUT SG::DataStructure::Common::Id& GC) const
{
    assert (0);
}

// void Cartesian2Coord::GetRefPoint (_OUT Id& ref, _OUT Point& PA, Point& PB, Point& PC)
// {
//     ref = m_ref;
//     PA  = m_PA;
//     PB  = m_PB;
//     PC  = m_PC;
// }

void Cartesian2Coord::GetRefPoint (_OUT SG::DataStructure::Common::Id& ref,
                                   _OUT SG::Algebra::Point& PA,
                                   _OUT SG::Algebra::Point& PB,
                                   _OUT SG::Algebra::Point& PC)
{
    ref = m_ref;
    PA  = m_PA;
    PB  = m_PB;
    PC  = m_PC;
}

Point Cartesian2Coord::Trans2Basic (const Real& localX, const Real& localY, const Real& localZ) const
{
    assert (Status_Initialized == m_status);

    Vector3D UP = m_bases[0] * localX + m_bases[1] * localY + m_bases[2] * localZ;
    return { m_origin.m_x + UP.m_x, m_origin.m_y + UP.m_y, m_origin.m_z + UP.m_z };
}

Cartesian2Coord::Cartesian2Coord ()
    : ICoordinate (SG::DataStructure::Common::DEFAULT_ID, SG::DataStructure::FEM::CoordTypes::COORDTYPE_Cartesian2)
{}

Cartesian2Coord::Cartesian2Coord (SG::DataStructure::Common::Id id,
                                  SG::DataStructure::Common::Id ref,
                                  const SG::Algebra::Point&     PA,
                                  const SG::Algebra::Point&     PB,
                                  const SG::Algebra::Point&     PC)
    : ICoordinate (id, SG::DataStructure::FEM::CoordTypes::COORDTYPE_Cartesian2), m_ref (ref), m_PA (PA), m_PB (PB), m_PC (PC)
{}

Cartesian2Coord SG::DataStructure::FEM::CORD2RData::GetCartesian2Coord ()
{
    return { m_id, m_rId, { m_a1, m_a2, m_a3 }, { m_b1, m_b2, m_b3 }, { m_c1, m_c2, m_c3 } };
}

/// @brief 判断两个CORD2RData是否相等
/// @param a
/// @param b
/// @return
bool SG::DataStructure::FEM::operator== (const CORD2RData& a, const CORD2RData& b)
{
    if (a.m_id != b.m_id)
        return false;
    if (a.m_rId != b.m_rId)
        return false;

    if (!SG::DataStructure::Common::isNear (a.m_a1, b.m_a1))
        return false;
    if (!SG::DataStructure::Common::isNear (a.m_a2, b.m_a2))
        return false;
    if (!SG::DataStructure::Common::isNear (a.m_a3, b.m_a3))
        return false;

    if (!SG::DataStructure::Common::isNear (a.m_b1, b.m_b1))
        return false;
    if (!SG::DataStructure::Common::isNear (a.m_b2, b.m_b2))
        return false;
    if (!SG::DataStructure::Common::isNear (a.m_b3, b.m_b3))
        return false;

    if (!SG::DataStructure::Common::isNear (a.m_c1, b.m_c1))
        return false;
    if (!SG::DataStructure::Common::isNear (a.m_c2, b.m_c2))
        return false;
    if (!SG::DataStructure::Common::isNear (a.m_c3, b.m_c3))
        return false;

    if (a.m_dominId != b.m_dominId)
        return false;

    return true;
}

SG::DataStructure::FEM::CORD2RData::CORD2RData (SG::DataStructure::Common::Id Id1,
                                                SG::DataStructure::Common::Id Index1,
                                                const SG::Algebra::Point&     PA1,
                                                const SG::Algebra::Point&     PB1,
                                                const SG::Algebra::Point&     PC1)
{
    m_id  = Id1;
    m_rId = Index1;
    m_a1  = PA1.m_x;
    m_a2  = PA1.m_y;
    m_a3  = PA1.m_z;
    m_b1  = PB1.m_x;
    m_b2  = PB1.m_y;
    m_b3  = PB1.m_z;
    m_c1  = PC1.m_x;
    m_c2  = PC1.m_y;
    m_c3  = PC1.m_z;
}